#include "App_Obstacle.h"
#include "Int_Buzzer.h"
#include "Int_Motor.h"
#include "Int_Radio.h"
#include "Int_OLED_Data.h"
#include "Dri_Timer_2.h"

/** 雷达探测返回距离 */
static u16 S_Radio_Distance = 0;
static u8 S_Radio_Count     = 0;

/**
 * @brief 定时器返回数据
 *
 */
void App_Obstacle_Timer_Back()
{
    // 如果距离短就停止
    S_Radio_Distance = Int_Radio_Start();
    // 小于 15Cm
    if (S_Radio_Distance <= 300) {
        Int_Motor_setLeft(0);
        Int_Motor_setRight(0);
    }
}

/**
 * @brief 雷达初始化
 *
 */
void App_Obstacle_Init()
{
    Dri_Timer_AddCallBack(App_Obstacle_Timer_Back);
}

/**
 * @brief 小车避障
 *
 */
void App_Obstacle_Controler()
{
    S_Radio_Distance = Int_Radio_Start();
    // 小于 15Cm
    if (S_Radio_Distance <= 200) {
        Int_BUzzer_Ring(100);
        if (S_Radio_Count == 0) {
            Int_Motor_setLeft(10);
            Int_Motor_setRight(-10);
            S_Radio_Count = 1;
            return;
        }
        if (S_Radio_Count == 1) {
            Int_Motor_setLeft(40);
            Int_Motor_setRight(-40);
            S_Radio_Count = 2;
            return;
        }
        if (S_Radio_Count == 2) {
            Int_Motor_setLeft(-40);
            Int_Motor_setRight(-40);
            S_Radio_Count = 1;
            return;
        }
    } else {
        S_Radio_Count = 0;
        Int_Motor_setLeft(40);
        Int_Motor_setRight(40);
    }
}